#include "imu_gps_node.hpp"
#include <iomanip>
#include "imu_gps_loc/base_type.h"

ImuGpsNode::ImuGpsNode(ros::NodeHandle& nh)
{
	initialized_ = false;
	// Load configs.
	/*double acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise;
	nh.param("acc_noise", acc_noise, 1e-2);
	nh.param("gyro_noise", gyro_noise, 1e-4);
	nh.param("acc_bias_noise", acc_bias_noise, 1e-6);
	nh.param("gyro_bias_noise", gyro_bias_noise, 1e-8);

	double x, y, z;
	nh.param("I_p_Gps_x", x, 0.);
	nh.param("I_p_Gps_y", y, 0.);
	nh.param("I_p_Gps_z", z, 0.);
	const Eigen::Vector3d I_p_Gps(x, y, z);

	std::string log_folder = "/home";
	ros::param::get("log_folder", log_folder);
	file_state_.open(log_folder + "/state.csv");
	file_gps_.open(log_folder + "/gps.csv");*/

	std::string config_file = "";
	ros::param::get("config_file", config_file);

	config_.LoadParameters(config_file);

	eskf_ptr_ = std::make_shared<ESKF>(config_);
	
	imu_sub_ = nh.subscribe("/imu/data", 10, &ImuGpsNode::ImuCallback, this);
	gps_sub_ = nh.subscribe("/fix", 10, &ImuGpsNode::GpsPositionCallback, this);

	state_pub_ = nh.advertise<nav_msgs::Path>("fused_path", 10);
}

ImuGpsNode::~ImuGpsNode()
{
	/*file_state_.close();
	file_gps_.close();*/
}

void ImuGpsNode::ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg)
{
	auto imu_data = std::make_shared<IMUData>();

	/*Eigen::Vector3d true_linear_accel = Eigen::Vector3d::Zero();
	Eigen::Vector3d true_angle_velocity = Eigen::Vector3d::Zero();
	Eigen::Quaterniond true_q_enu = Eigen::Quaterniond::Identity();
	Eigen::Vector3d true_t_enu = Eigen::Vector3d::Zero();*/

	imu_data->time = imu_msg->header.stamp.toSec();
	imu_data->linear_accel << imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z;
	imu_data->angle_velocity << imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z;

	if (!initialized_) {
		AddImuData(imu_data);
		return;
	}
	eskf_ptr_->Predict(*imu_data);
}

void ImuGpsNode::AddImuData(const ImuDataPtr imu_data_ptr)
{
	imu_buffer_.push_back(imu_data_ptr);
	if (imu_buffer_.size() > 100) {
		imu_buffer_.pop_front();
	}
}

//Eigen::Quaterniond(eskf_ptr_->GetPose().topLeftCorner<3, 3>())
//eskf_ptr_->GetPose().topRightCorner<3, 1>()

void ImuGpsNode::GpsPositionCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg)
{
	GPSDataPtr gps_data = std::make_shared<GPSData>();
	gps_data->time = gps_msg->header.stamp.toSec();
	gps_data->position_lla << gps_msg->latitude, gps_msg->longitude, gps_msg->altitude;
	//gps_data->cov = Eigen::Map<const Eigen::Matrix3d>(gps_msg->position_covariance.data());
	
	//Eigen::Vector3d position_lla = Eigen::Vector3d::Zero(); // LLA  unit: degree
	//Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); // NED
	//Eigen::Vector3d local_position_ned = Eigen::Vector3d::Zero();
	//Eigen::Vector3d true_velocity = Eigen::Vector3d::Zero();
	//Eigen::Vector3d true_position_lla = Eigen::Vector3d::Zero();

	Eigen::Matrix3d G_R_I;
	if (!initialized_)
	{
		if (!InitByGpsPositionData(gps_data, G_R_I)) {
			return;
		}
		initialized_ = true;
		//gps_processor_->init_lla_ = gps_data_ptr->lla;
		eskf_ptr_->Init(curr_gps_data_, curr_imu_data_);
		return;
	}
	eskf_ptr_->Correct(*gps_data);
}

void ImuGpsNode::InitByGpsPositionData(const GPSDataPtr gps_data_ptr, Eigen::Matrix3d& G_R_I)
{
	if (imu_buffer_.size() < 100) {
		LOG(WARNING) << "[AddGpsPositionData]: No enought imu data!";
		return false;
	}

	const ImuDataPtr last_imu_ptr = imu_buffer_.back();
	if (std::abs(gps_data_ptr->time - last_imu_ptr->time) > 0.5) {
		//LOG(ERROR) << "[AddGpsPositionData]: Gps and imu timestamps are not synchronized!";
		return false;
	}
	// We can use the direction of gravity to set roll and pitch. But, we cannot set the yaw. 
	// So, we set yaw to zero and give it a big covariance.
	if (!ComputeG_R_IFromImuData(&G_R_I)) {
		//LOG(WARNING) << "[AddGpsPositionData]: Failed to compute G_R_I!";
		return false;
	}
	return true;
}


bool ImuGpsNode::ComputeG_R_IFromImuData(Eigen::Matrix3d* G_R_I)
{
	// Compute mean and std of the imu buffer.
	Eigen::Vector3d sum_acc(0., 0., 0.);
	for (const auto imu_data : imu_buffer_) {
		sum_acc += imu_data->acc;
	}
	const Eigen::Vector3d mean_acc = sum_acc / (double)imu_buffer_.size();

	Eigen::Vector3d sum_err2(0., 0., 0.);
	for (const auto imu_data : imu_buffer_) {
		sum_err2 += (imu_data->acc - mean_acc).cwiseAbs2();
	}
	const Eigen::Vector3d std_acc = (sum_err2 / (double)imu_buffer_.size()).cwiseSqrt();

	if (std_acc.maxCoeff() > kAccStdLimit) {
		LOG(WARNING) << "[ComputeG_R_IFromImuData]: Too big acc std: " << std_acc.transpose();
		return false;
	}

	// Compute rotation. Please refer to 
	// https://github.com/rpng/open_vins/blob/master/ov_core/src/init/InertialInitializer.cpp

	// Three axises of the ENU frame in the IMU frame. z-axis.
	const Eigen::Vector3d& z_axis = mean_acc.normalized();

	// x-axis.
	Eigen::Vector3d x_axis = Eigen::Vector3d::UnitX() - z_axis * z_axis.transpose() * Eigen::Vector3d::UnitX();
	x_axis.normalize();

	// y-axis.
	Eigen::Vector3d y_axis = z_axis.cross(x_axis);
	y_axis.normalize();

	Eigen::Matrix3d I_R_G;
	I_R_G.block<3, 1>(0, 0) = x_axis;
	I_R_G.block<3, 1>(0, 1) = y_axis;
	I_R_G.block<3, 1>(0, 2) = z_axis;

	*G_R_I = I_R_G.transpose();
	return true;
}

//void ImuGpsNode::PublishState(const ImuGps::State& state, const ros::Time stamp)
//{
//	path_fusion_.header.frame_id = "world";
//	path_fusion_.header.stamp = stamp;
//	//path_fusion_.header.stamp = ros::Time::now();
//
//	geometry_msgs::PoseStamped pose;
//	pose.header = path_fusion_.header;
//
//	pose.pose.position.x = state.G_p_I[0];
//	pose.pose.position.y = state.G_p_I[1];
//	pose.pose.position.z = state.G_p_I[2];
//
//	const Eigen::Quaterniond G_q_I(state.G_R_I);
//	pose.pose.orientation.x = G_q_I.x();
//	pose.pose.orientation.y = G_q_I.y();
//	pose.pose.orientation.z = G_q_I.z();
//	pose.pose.orientation.w = G_q_I.w();
//
//	path_fusion_.poses.push_back(pose);
//	state_pub_.publish(path_fusion_);
//}

int main(int argc, char** argv)
{
	// Set glog.
	//FLAGS_colorlogtostderr = true;

	// Initialize ros.
	ros::init(argc, argv, "imu_gps_fusion");
	ros::NodeHandle nh;

	// Initialize localizer.
	ImuGpsNode localizer(nh);

	ros::spin();
	return 1;
}